/******************************************************************************/
/*  mbfuncdisc.c                                                              */
/*                                                                            */
/*  FreeModbus Libary: A portable Modbus implementation for Modbus ASCII/RTU. */
/*  Copyright (c) 2006 Christian Walter <wolti@sil.at>                        */
/*  All rights reserved.                                                      */
/******************************************************************************/

#include "data_types.h"

#include "port\port.h"
#include "mbconfig.h"
#include "mb.h"
#include "mbconfig.h"
#include "mbframe.h"
#include "mbproto.h"
#include "functions\mbutils.h"
#include "mbfunc.h"


/******************************************************************************/
/*      Defines                                                               */
/******************************************************************************/
#define MB_PDU_FUNC_READ_ADDR_OFF           (MB_PDU_DATA_OFF)
#define MB_PDU_FUNC_READ_DISCCNT_OFF        (MB_PDU_DATA_OFF + 2)
#define MB_PDU_FUNC_READ_SIZE               (4)
#define MB_PDU_FUNC_READ_DISCCNT_MAX        (0x07D0)



eMB_EXCEPTION prveMbError2Exception(const eMB_ERROR_CODE _errorCode);



#if MB_FUNC_READ_COILS_ENABLED > 0
//==============================================================================
// MbFuncReadDiscreteInputs()
//==============================================================================
eMB_EXCEPTION MbFuncReadDiscreteInputs(uint8_t * _pFrame, uint16_t * _pLen)
{
    uint16_t regAddress;
    uint16_t discreteCnt;
    uint8_t numBytes;
    uint8_t * pFrameCur;

    eMB_EXCEPTION status = MB_EX_NONE;
    eMB_ERROR_CODE regStatus;

    if (*_pLen == (MB_PDU_FUNC_READ_SIZE + MB_PDU_SIZE_MIN)) {
        regAddress = (uint16_t)(_pFrame[MB_PDU_FUNC_READ_ADDR_OFF] << 8);
        regAddress |= (uint16_t)(_pFrame[MB_PDU_FUNC_READ_ADDR_OFF + 1]);
        regAddress++;

        discreteCnt = (uint16_t)(_pFrame[MB_PDU_FUNC_READ_DISCCNT_OFF] << 8);
        discreteCnt |= (uint16_t)(_pFrame[MB_PDU_FUNC_READ_DISCCNT_OFF + 1]);

        // Check if the number of registers to read is valid. If not
        // return Modbus illegal data value exception.
        if ((discreteCnt >= 1) && (discreteCnt < MB_PDU_FUNC_READ_DISCCNT_MAX)) {
            // Set the current PDU data pointer to the beginning
            pFrameCur = &_pFrame[MB_PDU_FUNC_OFF];
            *_pLen = MB_PDU_FUNC_OFF;

            // First byte contains the function code
            *pFrameCur++ = MB_FUNC_READ_DISCRETE_INPUTS;
            *_pLen += 1;

            // Test if the quantity of coils is a multiple of 8. If not last
            // byte is only partially field with unused coils set to zero.
            if ((discreteCnt & 0x0007) != 0) {
                numBytes = (uint8_t) (discreteCnt / 8 + 1);
            }
            else {
                numBytes = (uint8_t) (discreteCnt / 8);
            };

            *pFrameCur++ = numBytes;
            *_pLen += 1;

            regStatus = ModbusRegDiscreteCb(pFrameCur, regAddress, discreteCnt);

            // If an error occured convert it into a Modbus exception
            if (regStatus != MB_ENOERR) {
                status = prveMbError2Exception(regStatus);
            }
            else {
                /*  The response contains the function code, the starting address
                 *  and the quantity of registers. We reuse the old values in the 
                 *  buffer because they are still valid.
                 */
                *_pLen += numBytes;;
            };
        }
        else {
            status = MB_EX_ILLEGAL_DATA_VALUE;
        };
    }
    else {
        // Can't be a valid read coil register request because the length
        // is incorrect.
        status = MB_EX_ILLEGAL_DATA_VALUE;
    };

    return status;
}
//==============================================================================
//==============================================================================
#endif